/**
 * @file KortstePad.cpp
 * @author Bram Aerts
 * @brief Provides implementation of shortest path algorithm.
 */

#include "KortstePad.h"
#include "Point.h"
#include <math.h>
#include <iostream>

KortstePad::KortstePad(Point s,Point d,std::vector<Point> o)
{
	src=s;
	dst=d;
	obj=o;
}
std::vector<Point> KortstePad::getWeg(void){
	PadEntry *Root = new PadEntry(src);

	Root->setVolgende(new PadEntry(dst));	

	int kost=recWeg(src,dst,Root);
	std::cout<<"kost: "<<kost<<"\n";

	//Root->printUnder();
	

	std::vector<Point>Oplossing;
	PadEntry *tmp;
	while (Root!=NULL){
		Oplossing.push_back(Root->getPoint());
		tmp=Root->getVolgende();
		delete Root;
		Root=tmp;
	}
	for(std::vector<int>::size_type i = 0; i != Oplossing.size(); i++)
		std::cout<<Oplossing[i].getX()<<" "<<Oplossing[i].getY()<<"\n";
	return Oplossing;
}

int KortstePad::recWeg(Point s,Point d,PadEntry* Root){
	double m,q,q2,q1;
	double tmp;
	double EenAfstand=60;
	double EenDraai=80;
	int bovengrens=479;
	int ondergrens=0;
	int linksgrens=0;
	int rechtgrens=639;
	int kost=0;
	Point tussenPoint;
	//std::cout<<"check 1:nieuwe weg\n";	std::cout<<"source: ";s.printPoint();	std::cout<<"destin: ";d.printPoint();
	//std::cout<<"de lijst ziet er als volgt uit: \n";Root->printUnder();
	
	m=(double)( s.getY()-d.getY() ) / (double)( s.getX()-d.getX() );	//voorschrift van rechte berekenen
	q=s.getY() - m*s.getX();											//
	/*als er op je pad een voorwerp staat bereken je alternatieve route*/
	for(std::vector<int>::size_type i = 0; i != getObj().size(); i++) {		
	  if ( 	(-m *(double)(getObj() [i].getX()) + (double)(getObj() [i].getY()) -q)* 	//als niet op de lijn ligt
		   	(-m *(double)(getObj() [i].getX()) + (double)(getObj() [i].getY()) -q)/
			(m*m+1) > EenAfstand*EenAfstand ){
		continue;
	  }			
	  else if(((s.getX()-		  d.getX())*(s.getX()-		    d.getX())+(s.getY()-		  d.getY())*(s.getY()-		    d.getY()) < //als niet tussen beide Pointen ligt
			  (s.getX()-getObj()[i].getX())*(s.getX()-getObj()[i].getX())+(s.getY()-getObj()[i].getY())*(s.getY()-getObj()[i].getY()) ) ||
              (s.getX()-		  d.getX())*(s.getX()-		    d.getX())+(s.getY()-		  d.getY())*(s.getY()-		    d.getY()) < 
			  (d.getX()-getObj()[i].getX())*(d.getX()-getObj()[i].getX())+(d.getY()-getObj()[i].getY())*(d.getY()-getObj()[i].getY()) ) {
	    continue;	  
	  }
	   
		/*het eerste object dat in de weg ligt:	bereken nieuw te berekenen Point voor het pad*/	
	  	q2= getObj() [i].getY() - m*getObj() [i].getX();/*q van lijn door middelPoint object*/
		

		int k=100;
		int grens=0;
		q1=q2;
		while(k>=20){
			k=100;
			q2= getObj() [i].getY() - m*getObj()[i].getX();
			
			if (q1==q2){/*de eerste keer*/
	  			if (q2<q)	/*je moet verder naar onder verplaatsen*/	
					q1=q2+EenDraai/sqrt(1+m*m)+5;
				else		/*verplaats verder naar boven*/
					q1=q2-EenDraai/sqrt(1+m*m)-5;
			}
			else{
				if (q2>q1)
					q1=q2+(q2-q1)+10;//je spiegelt telkens, als je naar boven gaat verhoog je met 10
				else
					q1=q2+(q2-q1);
			}
			std::cout<<"("<<s.getX()<<","<<s.getY()<<") ("<<d.getX()<<","<<d.getY()<<")  ";
			
	  		q2= getObj() [i].getY() + 1/m*getObj() [i].getX(); /*q van gespiegelde lijn door middelPoint object*/	
			
			//HIER ZAT FOUT
			tussenPoint=Point((int)((q2-q1)/(m+1/m)),(int)((q2*m*m+q1)/(m*m+1)));/*snijPoint van loodrechte lijn en het Point op afstand */
			std::cout<<"  tp: "<<tussenPoint.getX()<<","<<tussenPoint.getY();
			if (tussenPoint.getX()>rechtgrens||tussenPoint.getX()<linksgrens||tussenPoint.getY()>bovengrens||tussenPoint.getY()<ondergrens){
				grens++;
				if (grens>2)	
					
				k=20;continue;
			}
			grens=0;	
			for(std::vector<int>::size_type j = 0; j != getObj().size(); j++) {			//kijk of er geen object te dicht bij het draaiPoint ligt
				tmp=(getObj()[j].getY()-tussenPoint.getY())*(getObj()[j].getY()-tussenPoint.getY())+
					(getObj()[j].getX()-tussenPoint.getX())*(getObj()[j].getX()-tussenPoint.getX());
				if (tmp < EenDraai*EenDraai){
						std::cout<<"   obj"<<j<<" "<<tmp<<"\n";
						k=20;break;
				}
			}	
			if( k==20){
				continue;
			}
			else 
				std::cout<<"  dit gaat \n";
			//Root->printUnder();
		  	kost+=2;//draai kost meer tijd
			
				//std::cout<<"hoofdprogramma: \n van: ";s.printPoint();std::cout<<"naar: ";d.printPoint();
				//std::cout<<"de lijst ziet er als volgt uit: \n"; Root->printUnder();

			PadEntry *van=Root->FindNode(s);			//
			PadEntry *tussen=new PadEntry(tussenPoint);	//
			tussen->setVolgende(van->getVolgende());	//dit is de enige plaats waar je de lijst aanpast
			van->setVolgende(tussen);					//

				//std::cout<<"**hoofdprogramma: \n van: ";s.printPoint();std::cout<<"naar: ";d.printPoint();
				//std::cout<<"de lijst ziet er als volgt uit: \n"; Root->printUnder();
					
			kost+=recWeg(s,tussenPoint,Root);
				
			kost+=recWeg(tussenPoint,d,Root);	
			if (kost>15);//VERWIJDER TERUG ALLE TUSSENGEVOEGDE PUNTEN EN PROBEER MET ANDER TUSSENPUNT
			k=kost;
			std::cout<<kost<<"\n";
		}

	  	return kost;
	}
		return kost+1;
}
